import pyrealsense2 as rs
import json  # 用于JSON文件操作

def get_camera_intrinsics(serial_number, width=1280, height=720):
    # 初始化相机管道和配置（与之前逻辑一致）
    pipeline = rs.pipeline()
    config = rs.config()

    # 筛选指定序列号的设备
    ctx = rs.context()
    target_device = None
    for dev in ctx.query_devices():
        if dev.get_info(rs.camera_info.serial_number) == serial_number:
            target_device = dev
            break
    if not target_device:
        raise ValueError(f"未找到序列号为 {serial_number} 的设备，请检查连接或序列号")

    # 配置流参数（彩色流，1280x720）
    config.enable_device(serial_number)
    config.enable_stream(
        rs.stream.color,  # 若需深度流内参，改为 rs.stream.depth，格式改为 rs.format.z16
        width, height,
        rs.format.bgr8,
        30
    )

    # 启动管道并获取内参
    pipeline_profile = pipeline.start(config)
    try:
        stream_profile = pipeline_profile.get_stream(rs.stream.color)
        intrinsics = stream_profile.as_video_stream_profile().get_intrinsics()

        # 整理内参为字典
        intrinsics_dict = {
            "fx": intrinsics.fx,
            "fy": intrinsics.fy,
            "cx": intrinsics.ppx,
            "cy": intrinsics.ppy,
            "width": intrinsics.width,
            "height": intrinsics.height
        }

        return intrinsics_dict

    finally:
        pipeline.stop()

def save_intrinsics_to_json(intrinsics, output_path="inputs/camera_intrinsics.json"):
    """将内参字典保存为JSON文件"""
    try:
        with open(output_path, "w", encoding="utf-8") as f:
            # indent=4 用于格式化JSON，便于阅读
            json.dump(intrinsics, f, indent=4, ensure_ascii=False)
        print(f"内参已成功保存至：{output_path}")
    except IOError as e:
        print(f"保存JSON文件失败：{e}")

if __name__ == "__main__":
    camera_serial = "318122301415"
    try:
        # 获取1280x720分辨率下的内参
        intrinsics = get_camera_intrinsics(camera_serial, width=1280, height=720)
        # 保存为JSON文件（可自定义路径，如 output_path="my_intrinsics.json"）
        save_intrinsics_to_json(intrinsics)
    except Exception as e:
        print(f"错误：{e}")
